#include<iostream>
#include<math.h>
#include<vector>
#include<string>
#include<fstream>
#include<opencv2/opencv.hpp>
#include<yaml-cpp/node/parse.h>
#include "yaml-cpp/yaml.h"
#include<GeographicLib/Geodesic.hpp>
#include<GeographicLib/Geocentric.hpp>
#include<GeographicLib/Constants.hpp>

using namespace GeographicLib;
using namespace std;
using namespace cv;

#define PI 3.1415926
#define WINDOW "原图"

vector<Point2f> pixel_point;
Mat g_srcImage,g_dstImage;
Point previousPoint;

bool P = false;

void On_mouse(int event, int x, int y, int flags, void* ustc);


double angle_to_radian(double degree)
{
	double flag =(degree <0)? -1.0: 1.0;
	if(degree<0)
	{
		degree = degree*(-1.0);
	}
	double result = flag * (degree*PI)/180;
	return result;
}


void On_mouse(int event, int x, int y, int flags, void* ustc)     //get pixel point
{
	//vector<Point2f> out;
	if (event == EVENT_LBUTTONDOWN)
	{
	  previousPoint = Point(x, y);
		char Txt_Point[80];
	  sprintf(Txt_Point,"(%d,%d)",x,y);
		putText(g_srcImage,Txt_Point,previousPoint,FONT_HERSHEY_COMPLEX, 0.5, Scalar(0, 0, 0, 255), 1, 8);
		circle(g_srcImage,previousPoint,3,Scalar(0,0,255),2);
		imshow(WINDOW,g_srcImage);
		//cout<<previousPoint.x<<" "<<previousPoint.y<<endl;
		
		//out=get_pixel_point(previousPoint);
		pixel_point.push_back(Point2f(previousPoint.x,previousPoint.y));
	} 
}

vector<Point3f> wgs2xy(vector<double>&gps)
{
 	vector<Point3f>xy;
	Geodesic geod(Constants::WGS84_a(), Constants::WGS84_f());

	double s12, azi1,azi2;
	/**
	double
      lat0 = 31.42807566666667, lon0 = 120.63983669444444, // JFK Airport
      lat1 = 31.42800188888889, lon1 = 120.64001433333334,  // LHR Airport
			lat2 = 31.427956944444446,lon2 = 120.64011872222223,
			lat3 = 31.4279025,        lon3 = 120.6401241388889,
			lat4 = 31.427810416666667,lon4 = 120.64008777777778,
			lat5 = 31.42787211111111 ,lon5 = 120.64000944444444,
			lat6 = 31.427914055555554,lon6 = 120.64007505555556,
			lat7 = 31.427985694444445,lon7 = 120.64001091666667,
			lat8 = 31.427970583333334,lon8 = 120.63996144444444;
	**/
	double 
			lat0 = gps[0] , lon0 = gps[1],
			lat1 = gps[2] , lon1 = gps[3],
			lat2 = gps[4] , lon2 = gps[5],
			lat3 = gps[6] , lon3 = gps[7],
			lat4 = gps[8] , lon4 = gps[9],
			lat5 = gps[10], lon5 = gps[11],
			lat6 = gps[12], lon6 = gps[13],
			lat7 = gps[14], lon7 = gps[15],
			lat8 = gps[16], lon8 = gps[17];

  geod.Inverse(lat0, lon0, lat1, lon1, s12,azi1,azi2);   ///azi1  azi2  is arcsin
  double x1 = s12 * sin(angle_to_radian(azi2));
	double y1 = s12 * cos(angle_to_radian(azi2));
	xy.push_back(Point3f(x1,y1,0));

	geod.Inverse(lat0, lon0, lat2, lon2, s12,azi1,azi2);
	double x2 = s12 * sin(angle_to_radian(azi2));
	double y2 = s12 * cos(angle_to_radian(azi2));
	xy.push_back(Point3f(x2,y2,0));

	geod.Inverse(lat0, lon0, lat3, lon3, s12,azi1,azi2);
	double x3 = s12 * sin(angle_to_radian(azi2));
	double y3 = s12 * cos(angle_to_radian(azi2));
	xy.push_back(Point3f(x3,y3,0));
	
	geod.Inverse(lat0, lon0, lat4, lon4, s12,azi1,azi2);
	double x4 = s12 * sin(angle_to_radian(azi2));
	double y4 = s12 * cos(angle_to_radian(azi2));
	xy.push_back(Point3f(x4,y4,0));

	geod.Inverse(lat0, lon0, lat5, lon5, s12,azi1,azi2);
	double x5 = s12 * sin(angle_to_radian(azi2));
	double y5 = s12 * cos(angle_to_radian(azi2));
	xy.push_back(Point3f(x5,y5,0));

	geod.Inverse(lat0, lon0, lat6, lon6, s12,azi1,azi2);
	double x6 = s12 * sin(angle_to_radian(azi2));
	double y6 = s12 * cos(angle_to_radian(azi2));
	xy.push_back(Point3f(x6,y6,0));
	
	geod.Inverse(lat0, lon0, lat7, lon7, s12,azi1,azi2);
	double x7 = s12 * sin(angle_to_radian(azi2));
	double y7 = s12 * cos(angle_to_radian(azi2));
	xy.push_back(Point3f(x7,y7,0));
	
	geod.Inverse(lat0, lon0, lat8, lon8, s12,azi1,azi2);
	double x8 = s12 * sin(angle_to_radian(azi2));
	double y8 = s12 * cos(angle_to_radian(azi2));
	xy.push_back(Point3f(x8,y8,0));
	
	cout<<"xy:"<<xy<<endl;
	return xy;
}
//Mat 2 vector
vector<double> convertMat2Vector(const Mat &mat)
{
	return (vector<double>)(mat.reshape(1, 1));//通道数不变，按行转为一行
}

int main(int argc,char *argv[]) 
{

		YAML::Node config;
		YAML::Node config1;
		try
	  {
    	config = YAML::LoadFile("out_camera_data.yml");
			config1 = YAML::LoadFile("../config/pos.yml");
   	} 
		catch (...) 
		{
    	printf("error loading file, yaml file error or not exist.\n");
    	return 0;
   	}

		string path = "../../out_img/*.jpg";
		vector<string>pictures;
		cv::glob(path,pictures,false);
		for(auto img : pictures)
		{
			cout<<"img: "<<img<<endl;
			g_srcImage = imread(img);
			imshow(WINDOW, g_srcImage);
			while(1)
			{
				setMouseCallback(WINDOW, On_mouse, (void*)(&g_srcImage));
				if(waitKey(1000) == 113)  break;
			}
  	}

		std::vector<double> v_cam  = config["camera_matrix"]["data"].as<std::vector<double>>();
		std::vector<double> v_dist = config["distortion_coefficients"]["data"].as<std::vector<double>>();

		double camD[9];
		double distCoeffD[5];
		for(auto &cam:v_cam)
		{
			static int i=0;
			camD[i++] = cam;
		}
		for(auto &dist:v_dist)
		{	
			static int i=0;
			distCoeffD[i++] = dist;
		}
		

		Mat camera_matrix = Mat(3,3 ,CV_64FC1,camD);
		Mat camera_discoefficients = Mat(5,1,CV_64FC1,distCoeffD);

		Mat rvec(3,1,cv::DataType<double>::type);
		Mat tvec(3,1,cv::DataType<double>::type);
		
		vector<Point3f>world_point;
		vector<double> gps_pos = config1["gps"].as<std::vector<double>>();   

		world_point = wgs2xy(gps_pos);               
		//out_pixel_point = get_pixel_point();

		solvePnP(world_point, pixel_point, camera_matrix, camera_discoefficients,rvec,tvec);
		
		//string filename = "camera.yml";
		//cv::FileStorage fs(filename,cv::FileStorage::WRITE);
		ofstream fout("camera.yml");
		YAML:: Node camera = YAML::LoadFile("camera.yml");

		camera["tvec"]= convertMat2Vector(tvec);
		camera["rvec"]= convertMat2Vector(rvec);
		camera["distortion_coefficients"] =  convertMat2Vector(camera_discoefficients);
		camera["camera_matrix"] =  convertMat2Vector( camera_matrix);
		fout<<camera;


		cout<<"all the pixel point as follow: "<<pixel_point<<endl;
		cout<<"the rvec is : "<<rvec<<endl;
		cout<<"the tvec is : "<<tvec<<endl;

		return 0;
}
